#include <stdio.h>
#include <math.h>
#define KP 0.05
#define KI 0.55
#define KD 0.07

typedef struct Indicator {
    float ActiveValue; // 当前值
    float TargetValue; // 目标值
    float OutputMax;   // 输出限幅
    float CurrError;   // k项偏差
    float LastError;   // k-1项偏差
    float PrevError;   // k-2项偏差
    float Increment;   // 输出增量
    float Transform;   // 输出转换值
} PID_Indicator;

typedef struct Indicator_ {
    float UPhotoresistor; // 上
    float DPhotoresistor; // 下
    float LPhotoresistor; // 左
    float RPhotoresistor; // 右
    float ActiveValue;    // 当前值
    float TargetValue;    // 目标值
    float OutputMax;      // 输出限幅
    float Increment;      // 输出增量
} SYS_Indicator;

PID_Indicator PID_initial(void)
{
    PID_Indicator indicator;
    indicator.ActiveValue = 0;
    indicator.TargetValue = 0;
    indicator.OutputMax   = 0;
    indicator.CurrError   = 0;
    indicator.LastError   = 0;
    indicator.PrevError   = 0;
    indicator.Increment   = 0;
    indicator.Transform   = 0;
    return indicator;
}

/**
 * @brief 设置算法输出限幅，代入当前值与设定值
 * @param indicator 
 * @param ActiveValue 
 * @param TargetValue 
 * @param OutputMax 
 */
void PID_setParameters(PID_Indicator *indicator, float ActiveValue, float TargetValue, float OutputMax)
{
    indicator->ActiveValue = ActiveValue;
    indicator->TargetValue = TargetValue;
    indicator->OutputMax   = OutputMax;
    indicator->CurrError   = 0;
    indicator->LastError   = 0;
    indicator->PrevError   = 0;
    indicator->Increment   = 0;
    indicator->Transform   = 0;
}

/**
 * @brief 增量式PID算法核心公式
 * @param indicator 
 * @return float 
 */
float PID_calculate(PID_Indicator *indicator)
{
    float Output = 0;

    indicator->PrevError = indicator->LastError;
    indicator->LastError = indicator->CurrError;
    indicator->CurrError = indicator->TargetValue - indicator->ActiveValue;

    float Proportion   = KP * (indicator->CurrError - indicator->LastError);
    float Intergral    = KI * indicator->CurrError;
    float Differential = KD * (indicator->CurrError - 2 * indicator->LastError + indicator->PrevError);
    float Increment    = Proportion + Intergral + Differential;

    Output = Increment > indicator->OutputMax ? indicator->OutputMax : Increment;
    Output = Output < -indicator->OutputMax ? -indicator->OutputMax : Output;

    return Output;
}

/**
 * @brief 转换算法输出增量值函数
 * @param indicator 
 * @return float 
 */
float PID_transform(PID_Indicator *indicator)
{
    float TransformValue;
    TransformValue = ((int)(((-indicator->Increment / 40) + 15) * 10 + 0.5)) / 10.0;
    TransformValue = (TransformValue < 15 && TransformValue > 14.5) ? 14.5 : TransformValue;
    TransformValue = (TransformValue > 15 && TransformValue < 15.5) ? 15.5 : TransformValue;

    return TransformValue;
}

/**
 * @brief 增量式PID算法调用函数
 * @param indicator 
 */
void PID_algorithmCall(PID_Indicator *indicator)
{
    indicator->Increment   = PID_calculate(indicator);                      // 算法公式计算
    indicator->ActiveValue = indicator->Increment + indicator->ActiveValue; // 更新当前值
    indicator->Transform   = PID_transform(indicator);                      // 算法输出值转换
}
